from machine import Pin, PWM
import time
from config import *

# 初始化l298n电机驱动模块引脚
motor_pin1 = PWM(Pin(MOTOR_PIN1), Pin.OUT)
motor_pin2 = PWM(Pin(MOTOR_PIN2), Pin.OUT)
# 初始化PWM引脚
servo_pin = PWM(Pin(SERVO_PIN), freq=SERVO_FREQ)
def set_servo_angle(angle):
    # 角度限制
    if angle < SERVO_MIN_ANGLE:
        angle = SERVO_MIN_ANGLE
    elif angle > SERVO_MAX_ANGLE:
        angle = SERVO_MAX_ANGLE
    
    # 计算脉宽
    pulse_width = SERVO_MIN_PULSE + (angle / SERVO_MAX_ANGLE) * (SERVO_MAX_PULSE - SERVO_MIN_PULSE)
    duty = int((pulse_width / 20) * PWM_RESOLUTION)
    servo_pin.duty(duty)  # 设置PWM输出

# 初始化当前角度
current_angle = INITIAL_ANGLE

# 主程序
def move_front():
    # 控制电机前进
    # motor_pin1.duty(512)  # 设置PWM输出占空比为50%
    # motor_pin2.duty(0)
    # time.sleep(2)
    # 控制电机前进
    motor_pin1.value(1)
    motor_pin2.value(0)
def move_back():
    # 控制电机后退
    # motor_pin1.duty(0)  # 设置PWM输出占空比为50%
    # motor_pin2.duty(512)
    motor_pin1.value(0)
    motor_pin2.value(1)
def stop():
    # 停止电机
    motor_pin1.value(0)  # 设置PWM输出占空比为0%
    motor_pin2.value(0)